/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
#include "RayCluster.h"
#include "mathutils.h"
#include <GL/gl.h>
#include "owndebug.h"


CRayCluster::CRayCluster(CObject *ParentObject, dSpaceID SpaceID,
dBodyID parentBodyID, dReal *posOffset, dMatrix3 rotOffset, int numRays, float range, float angle1, float angle2)
:CPrimitive(ParentObject,SpaceID,parentBodyID,NULL,false),
IsVisible(false),
numRays(numRays),
range(range),
angle1(angle1),
angle2(angle2),
coords(numRays,std::vector<float>(2))
{
  for (int i=0; i<12;i++)
    this->rotOffset[i]=rotOffset[i];

  this->posOffset[0]=posOffset[0];
  this->posOffset[1]=posOffset[1];
  this->posOffset[2]=posOffset[2];

  if (this->range<0)
    this->range=0;

  
  float step=(angle2-angle1)/(numRays-1);

  for (int i=0;i<numRays;i++)
  {

    dGeomID r=dCreateRay(SpaceID,range);
    //pi/2 has to be added as real j2b2's zero-angle is on center
    coords[i][0]=-cos(angle1+step*i+M_PI/2); //x has to be inverted to put positive x axis on the right hand side of scanner
    coords[i][1]=sin(angle1+step*i+M_PI/2);
    dGeomRaySet (r, 0, 0, 0, coords[i][0], 0, coords[i][1]);

    TGeomData *gd=new TGeomData;
    TRayInfo *ri=new TRayInfo;
    ri->index=i; ri->angle=angle1+step*i; ri->range=range;
    RayInfos.push_back(ri);

    gd->Primitive=this;
    gd->userData=ri;
    dGeomSetData(r,gd);
    rays.push_back(r);

  }
}

CRayCluster::~CRayCluster()
{
  std::vector<dGeomID>::iterator i;
  for (i=rays.begin();i!=rays.end();i++)
  {
    TGeomData *gd=(TGeomData*)dGeomGetData(*i);
    delete (TRayInfo*)gd->userData;
    delete gd;
  }
}

void CRayCluster::Set()
{

  const dReal *P, *R;
  float P2[3];
  float c[3];
  float r1[3];
  float r2[3];
  P=dBodyGetPosition(BodyID);
  R=dBodyGetRotation(BodyID);
  RotateVector(R,posOffset,P2);
  P2[0]+=P[0];
  P2[1]+=P[1];
  P2[2]+=P[2];

  for (int i=0;i<numRays;i++)
  {
    c[0]=coords[i][0];
    c[1]=0;
    c[2]=coords[i][1];
    RotateVector(rotOffset,c,r1);
    RotateVector(R,r1,r2);

    dGeomRaySet(rays[i], P2[0], P2[1], P2[2], r2[0], r2[1], r2[2]);
  }

}

void CRayCluster::Draw()
{
  if (IsVisible)
  {

    dVector3 P, D;
    glPushMatrix();

    glColor3f(0.8,0,0);
    for (std::vector<dGeomID>::iterator i=rays.begin();i!=rays.end();i++)
    {
      
        dGeomRayGet(*i,P,D);
        glBegin(GL_LINES);
          glVertex3f(P[0],P[1],P[2]);
          glVertex3f(P[0]+D[0]*range,P[1]+D[1]*range,P[2]+D[2]*range);
          //glVertex3f(0,0,0);
          //glVertex3f(0,0,range);
        glEnd();

    }
    glPopMatrix();
  }
}

const std::vector<TRayInfo*>* CRayCluster::GetRayInfo()
{
  return &RayInfos;
}
